AN IMU-AIDED CARRIER-PHASE DIFFERENTIAL GPS POSITIONING SYSTEM 



1 



An IMU- Aided Carrier-Phase Differential GPS 

Positioning System 

Shuqing Zeng, Member, IEEE 



(N 

o 

C 

in 

O 
^. 

o 



> 
in 

O 

o 

(N 



X 



Abstract — We consider the problem of carrier-phase differ- 
ential GPS positioning for an land vehicle navigation system 
(LVNS), tightly coupled with an inertial measurement unit (IMU) 
and a speedometer. The primary focus is to apply Bayesian net- 
work to an IMU-aided GPS positioning system based on carrier- 
phase differential GPS. We describe the implementation details 
of the positioning system that integrates GPS measurements (i.e., 
pseudo-range, carrier-phase and doppler), IMU measurements, 
and speedometer measurements. We derive the linearized state 
process equation and the measurement equation for GPS and 
speedometer. To account for constraints of land vehicle, we add 
two more pseudo measurements to ensure the perpendicular 
velocities close to zero. 

Index Terms — Differential Carrier-phase GPS, Land Vehicle 
Navigation System, IMU aided GNSS, Bayesian Network 



I. INTRODUCTION 

Global navigation satellite system such as GPS based 
positioning systems are in widespread use world-wide. It 
is possible to determine the position as accurate as a few 
centimeters if a differential configuration using a fixed known 
base station is applied. However this GPS system configure 
requires line-of-sight to the satellites. In urban areas with 
high buildings or in forests, the quality of the position 
estimate degrades due to multi-path effects or even leads to a 
signal outage (e.g., in tunnels or under the bridges). Another 
drawback of GPS based system is the slow update rate of GPS 
measurements. For applications such as autonomous driving, 
a more frequent estimation of vehicle position, velocity, and 
attitude is required. 

Inertial measurement unit (IMU) can provide such desired 
information for autonomous driving. Using accelerometers 
and gyroscopes, and Newton's law of motion, IMU can deter- 
mine the position, velocity, and attitude of the vehicle. IMU 
is a self-contained sensor and provides inertial measurement 
at a higher rate (e.g., 100 Hz for consumer grade devices). 
Since IMU measures the relative increment from the previous 
known state, a integration process (call dead-reckoning) is 
needed. Because of this integration, errors caused by sensor 
bias, sensor scale factor, and sensor nonlinearity are accu- 
mulated, and may yield unbounded drifts of the position and 
attitude estimation of the vehicle. 

Fusion systems integrating GPS with global accuracy and 
an IMU with local accuracy becomes the mainstream technol- 
ogy for land vehicle navigation system (LVNS) |fL3l . The GPS 
measurement aids the integration such that the drifting errors 
are bounded and, on the other hand, the IMU measurement 
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can be used to estimate GPS carrier phase cycle, and identify 
and correct cycle estimation error when cycle slip occurs. 

In this paper the primary focus is to apply the Bayesian 
network (BN) proposed in lfl5l to an IMU-aided GPS posi- 
tioning system based on carrier-phase differential GPS. We 
describe the implementation details of the positioning system 
that integrates GPS measurements (i.e., pseudo-range, carrier- 
phase and doppler), IMU measurements, and speedometer 
measurements. We derive the linearized state process equation 
to express the evolution of the augmented vehicle state 
consisting of vehicular position, velocity, attitude, and error 
parameters of IMU measurement (e.g., bias and scale factor). 
Also the measurement equation for GPS measurement is 
derived in term of the augmented state vector. To account 
for land vehicle that does not slip and travels along the bore- 
sight, we add two more pseudo measurements to ensure the 
perpendicular velocities are close to zero. 

Integration of GPS and IMU is a well-studied area J3], 0, 
0, H-HOl and successfully used in practice Q, fl2). Due to 
the fact that LVNS typically has to operate in areas where GPS 
signals are either blocked or severely degraded, ambiguity 
resolution (AR) of double-difference carrier phase data as 
integers is still a challenge problem. A few tens of seconds 
of data is required for AR to converge to a correct solution. 
However, the time between two consecutive dropouts for 
satellite may be much shorter than this requirement duration. 
Therefore, the AR process may be prematurely terminated 
due to outages, and new AR ones need to be started on-the- 
fly when satellites arise in the GPS-adverse environment. 

The rest of this paper is organized as follows. Section [TT] 
is devoted to the details of IMU data processing. Section [Til] 
is focused on GPS data processing. Section |IV] outlines the 
stochastic model of the sensor errors. In Section fVlwe discuss 
the algorithm to integrate data from IMU, GPS, and vehicle 
speedometer for positioning and attitude estimation of land 
vehicle. Finally we give concluding remarks in Section [VT] 

II. IMU Data Processing 

A. Coordinate Frames 

We begin with the definition of the three coordinate sys- 
tems: earth centered earth fixed (ECEF) system, local geodetic 
system, and vehicle body centered system. As shown in 
Fig-E earth-centered earth fixed (ECEF) system has its origin 
attached to the center of the Earth and rotates with it. GPS 
measurements are measured in the ECEF system (e-frame). 
Inertial measurements are measured in earth-centered inertial 
system (ECI) or i-frame, and are the combined result of the 



Earth rotation and the vehicle ego-motion. Local geodetic 
system (rt-frame) has its origin coincident with the fixed 
ground based station, its x-axis always points to geodetic 
east, y-axis points to geodetic north, and z-axis completes 
the right-handed orthogonal frame. The rotation matrix from 
n-frame to e-frame can be written as 



o 




— c 



(1) 



For example, given a position r e in e-frame, we write the 
corresponding coordinate in n-frame as r" = i?"(r e — o") 
where i?" = (i?fJ T and o" is vector from the origin of e- 
frame to the origin of n-frame, expressed in e-frame. 




Fig. 1. Earth-centered earth-fixed (ECEF) coordinate system (e), 
earth-centered inertial coordinate system (i), and local geodetic 
coordinate system (n). A and tp are latitude and longitude of the 
origin of the local geodetic frame, respectively. u>f is the mean 
angular velocity of the Earth, and g is the gravity vector all expressed 
in e-frame. o™ is the vector from the origin of e-frame to the origin 
of n-frame. <jj\, g, and o™ are expressed in e-frame. 

Fig. |2 a) illustrates the vehicle centered system (u-frame). 
This frame has its origin at the center of gravity of the vehicle 
with its x-axis pointing in the forward direction, the z-axis 
up through the ceiling of the vehicle, and y-axis completes 
the right-handed orthogonal system. 

The rotation matrix from a-frame (any coordinate frame 
i, e, n, or v) to another coordinate system 6-frame can be 
derived by subsequently rotations in the three planes (see 
Fig. [2b)-(d)), i.e., first in the plane spanned by the x- 
and y-axis, then the one spanned by x- and z-axis, and 
finally the plane spanned by y- and z-axis. Mathematically, 
this rotation matrix can be expressed by three Euler angles 
01 = {tp, 9, <t>) T . Note that 9 b a = -9% and 



R, 



R x {-4>)Ry{-e)R z {-^) 



= | C^SgS0 - C0S^, 

c^c^se + S0S^, 



c^c^, + ses^s^, cos,/, 
-c,/,S0 + c^ses,/, cgc,p 



(2) 




(c) Rotation by pitch R v (6) 



(d) Rotation by yaw i? z (i/>) 



Fig. 2. (a) The vehicle centered coordinate frame (v). (b)-(d) Vehicle 
attitude defined by angles of roll <f), pitch 8, and yaw tp. 



Consider infinitesimal angles S0 b a = (5<fi, 69, Sip) T for the 
roll, pitch, and yaw motion, the corresponding rotation matrix 
can be approximated by R h a « I 3 — A©' where A0^ = 
[59^] x is the skew symmetric matrix representation of the 



rotation angles 9 a , i.e., 



A0, = 





5ip 



o 



o 



At time t, a vector p in a-frame can be expressed in 6-frame 
as q(t) = i?k(t)p. Now consider at time t + At, 

q(t + At) = R b a (t + At)p = (I 3 - A0^)^(t)p 

The time derivative of the R h a is defined as 

R b a (t + At)-R b a (t) 



lim 

At->0 

= lim 



A< 

(I 3 -A0M(t)-i?»(t) 



At 



lim 

At^O At 



±R b a (t) 



(3) 



h A 

where tt a = limA->o ^ t a is the skew symmetric matrix 
angular rate u) b a = (4>,9,i>) T , i.e., tt b a = [u> b ]x. 
The transpose of (0 is 

RS(t) = -(Sl b a R b a (t)) T =RI(t)Sl b a 



where (fl a ) 



b \T 



ft" 



where cq = cos(6) and sg = sin(0). 



Note that (01) is equivalent to 

K(t) = 



(4) 



(5) 



B. Navigation Equation 

Considering a point r' in inertial frame, by Newton's laws, 
we can have the following kinematical acceleration equation: 

F = g< + P (6) 

where g 1 is the gravitational acceleration and f 4 is the 
vehicle's acceleration in i-frame. 

Assuming the center of the vehicle locate at r™ in the local 
geodetic frame, we can express the corresponding position in 
the inertial frame by 



Rl 



K< (7) 



where o" is time invariant and is the vector pointing from 
the origin of e-frame to the origin of n-frame, represented in 
e-frame. Note that the rotation matrix R l n can be decomposed 
as R l n = R\R e n , and R e n is time invariant. Referring (0), we 
can write derivatives of Ri as 



Rl 



Rnili 

Rl, 0™l~2i 



where v™ is the vehicle velocity in n-frame; and P and 
ujI are vehicle acceleration and angular rate in u-frame, 
which is directly measured by the accelerometers and gyros, 
respectively. 

Note that -2H?v" and -(n?) 2 o£ in (O are the Coriolis 
and centrifugal terms induced by the rotation of the Earth, 
and J~2™ is defined in ©. 

III. GPS Data Processing 

In this section, we develop the processing necessary to 
use GPS measurements for relative positioning. Consider the 
reference point A (base station) and the rover point B (center 
of the receiving antenna in the vehicle) in e-frame (c.f., Sec- 
tion HB. Let r A = [X A ,Y A ,Z A ] T and r A = [X A ,Y A ,Z A ] T 
denote the position and velocity vectors of A, respectively; 
r B = [X B ,Y Bl Z B ] T and v B = [X B ,Y B ,Z B ] T denote the 
position and velocity vectors of B, respectively. The baseline 
vector can be written as 



r B - r A , 



r B - r A 



where 



1~2™ — R^tt^Rf, 



(8) 



is the skew symmetric matrix of Earth's rotation in local 
geodetic frame n, and Of is the skew symmetric matrix of 
the Earth's angular velocity u>f defined in Fig. Q] 

By differentiating Q twice with respect to time, we obtain 



R l n r n + 2Rin™r n + <fl^^(r" 

Rtv n + 2Rin?r n + iwa ,l o" 



O n e ) 



(9) 



where we assume ||o"| ^> ||r™||. 

Plugging (|6]l into (O and multiplying i?" to both sides, we 
can approximate §6$ to be 

g" + f» = f" + 2f%r n + {Vt^fol (10) 

where f" is the vehicle's acceleration in n-frame, and g" is 
the gravity vector, g" = (0, 0, -9.80665) T m/s 2 QJ. 

Now we consider the kinematics of the vehicle attitude, 
expressed as the rotation matrix i?" from u-frame to ?i-frame. 

K = RX (ID 

where the skew-symmetric matrix fi^ for the the rotation 
rates between the local geodetic and vehicle frames consists 
of the angular rates U3\ measured by the gyros and the Earth 
rotational rate in ?i-frame, i.e., uj v n = u)^ — R^wf- 

Note that the rotation matrix f?" can be expressed by roll- 
pitch-yaw angles 0" = (<f>,9,ip) T (c.f., ©). Referred to ©, 
we note that ( flTT i is equivalent to 



e, 



RZ w f 



(12) 



where wf = (0,0, 7.29211501 x 10~ 5 ) rad/s Q is the mean 
angular velocity of the Earth (c.f., Fig. [T). 

In summary, combining Eqs.jTOb and (flZb . we obtain the 
navigation equation in the first-order differential equations as 







v™ 




V™ 




-2(l?v n - (OH 2 o r e l + g" + H?f° 


(13) 
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Note that in this paper we use the reference point A as the 
origin of the n-frame. Namely, r A = o" is the origin of the 
n-frame in the e-frame. 

A. GPS Observations 

The three basic measurements of a GPS receiver from a 
satellite are code (pseudo-range), phase, and doppler. For 
short baseline relative positioning!] the accuracy could be 
substantially improved by having a receiver (reference) broad- 
cast its measurements to nearby receivers (rovers). Let *^ 
denote the measurement from the receiver f3 and the a-th 
satellite. Giving two receivers A (reference) and B, and two 
satellites j (reference), and k, we define the double-difference 
convention = *^ ' — *^ — *^ B + * A where the asterisk 
may be replaced by R, $, D, p, and p that correspond 
to pseudo-range measurement, phase measurement, Doppler 
measurement, geometric distance between receiver and satel- 
lite, and time rate of the geometric distance. Thus the double- 
difference measurements (c.f., ifTTI p. 460]) can be written as 



R 
cD 



AB 
(jk) 
AB 
(jk) 
AB 



f 



(Jk) 

Pab 

a (jk) 

Pab 

xUk) Ak) 
Pab + s>ab 



„(*) 
Vab 

A(a<& 



Vab 



l AB> 



■'AB 



Aj) 

SAB 



(14) 
d&15) 

(16) 



$^ and denote the double- 



where the symbols R^ B , 
differences of code, phase, doppler measurements between 
the rover receiver B and base receiver A, respectively; p^ B 
is the double-difference geometric distance p~[g = p^ — 
Pa — Ps^ + Pe* ; P AB is the time derivatives of p^g ; single- 
difference a a B is the ambiguity for the a-th satelliufl A and 

'This refers to a relative distance between base and vehicle of 10 km 
for single frequency or 50 km for dual frequency under most atmospheric 
conditions 1111 . 

2 at time step t corresponds to the K aj t-th component in the 

ambiguity vector a. 



/ are the carrier wavelength and frequency, respectively; c is 
the speed of light; single-differences rf^ B , £^g, and Q^ B are 
the corresponding measurement errors. 

We assume rf^g, Cab' an< ^ Cab are unbiased and inde- 
pendently distributed with Gaussian distribution for different 
satellites at different epochs (c.f., (|301>-(|32"1>). 

In ([PiTl -dToT). we consider only the single carrier frequency 
(/x = 1575.42MHz, Ai = c/fi) since most low-cost receivers 
only receive LI signals. The case of dual-frequency may 
easily be accommodated by adding three more measurements 
as CK-© with f 2 = 1227.60MHz and A 2 = c// 2 , resulting 
in six basic outputs and two ambiguities per satellite. Also in 
the similar fashion we can handle the wide lane combination. 



B. Relative Positioning 

Let bo denote the approximated baseline. Let r^' = 
[XW,YW,Z<fi\ T and r W = [X^,Y^ k \Z^] T denote 
the earth-rotation corrected positions of the j-th and fc-th 
satellites in the ECEF frame, respectively. Let Xb , Yb > ar, d 
Zb be the component values of the approximated position 
yb (i\b = ya + bo) for the unknown point B. Then, the 
approximated geometric distances between the point B and 
the satellites j and fc can be calculated as 

p% = - x Bo y + (yu) - v Bo y + (zu> - z Bo y 



The distances between the point A and satellites j and k can 
be calculated as 



pf 



^J(XU) - Xa) 2 + {YU) - Y A ) 2 + (ZU> - Z A ) 2 
sj (XW -X A ) 2 + (YW - Y A ) 2 + (ZW - Z A ) 2 



We define (f$ and nty 



m.0 as the range rate and the vector 
of unit length from the receiver B (J3 = A or B) to 
the i-th satellite, respectively. Let b be the approximated 
baseline velocity. Let r") and denote velocity vectors of 
satellites j and fc, respectively, and yb„ (tb = ?a + bo) the 
approximated velocity for the unknown point B. Then, the 

range rates p Ba , p Bo , and pf£' are computed as 



P Bq 
Pb 



U)\T n U) 



■Ik) /. 

Pa = (""A 



b U)f n U) 



When the models in (fT4l> - (fT~6b are considered, the only 
terms comprising unknowns in nonlinear form are p^ B and 
Pab- Here we outline how p^ B and pj[ B is linearized in 
term of b and b. 

Since ya and of the reference point A are known (using 
single point solution for the reference receiver), we can write 
the linearized p^j B in the neighborhood of bo using Taylor 
expansion as 



P f B ] 



(fc) 
Pb ~ 

Pab 



(fc) 
Pa 

HA 



Jj) 
Pa 



b ) +h.o.t. 



(17) 



where 



A 
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AB 



r x^-x Bo 




(fc) 

Pbo 


r u) 
Pb q 


yW-Yb 


Y^-Y Bo 




„U) 
Pb 






W 
Pb 


Pb 



ijk) (fc) (fc) (j) . (j) 

Pabo = Pbo - Pa ~ Pb + Pa 



and h.o.t. represents the higher order terms of Taylor expan- 
sion. 

Similarly, we can write p^ B as 

A W - a(*) A« _ aO) + AO) 

Pab — Pb -Pa Pb + Pa n o\ 
= « + « + («) T (b-bo) (18) 

where 
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Pb 

yW-Y Bo 


z<- 
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U) 
Pb 

z (j) ~z Bo 






pV 

fB 
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Pab - 


•(fc) 

PBo 


•(fc) 
-Pa " 


Pb (i + Pa 



x (k) -x Ba , xW-x A , x^-x Bo X^-X A 



w> 1 wr 

P Br , Pa 



+ 



y(fc> - Y B j. Y^)-Y A M y (j) -Y Bo _ YU) Y A j y a 



Jk) T (fc) -1 JJ)- 



C. Measurement Matrix 

Suppose there are M visible satellites. Without loss of gen- 
erality we choose satellite 1 as the reference satellite (i.e., j = 
1). We define J = [ e I M -i } = [ J$ ••• Jji ] T 
with e = [1, 1] T and 1m -l an identity matrix. One can 



M-l 



verify that J is a (M — 1) x M matrix and its columns are 
linearly dependent. We also define 



y = [ b b 

a = [ a\ B 



M 
AB 



Er = 



/ a (1M)n T n 
\ A AB ) U l 







1x3 



X3 



E 



D 



'1x3 



'1x3 



(IMKt 

ABn ) 



or = 



0$ 



OD = 



R 



(12) J12) , /a (12) , r , 
" PaB + ( A AB J b 



I? 



,113 



1M) 
AS 



(1M) 
^AB 



(1M)^ T 
AB ( 



^ AB _ 



(12) 

Pabq 



(A 



I MB, 



b 
b 



(1M) 
AB 



P AB Q ^ 
.(12) 

Pab q 



(A 

- / 



(1M)^ T 
AB 

(12) 
A Bo ' 



(12) \Ti 



08. 



ABn 



.(1M) ,(1M) 

Pab — 4 < 



/ FAB l AB 

Ignoring the h.o.t. terms in (fTTt and plugging (fTTt and ([T8l l 
into (fPfll-tfToT), we combine M— 1 double-differences for code, 
phase, and doppler measurements as 



or = E R y + v v 

o<j> = Ery + A Ja + i/£ 

od = E D y + vq 



(19) 
(20) 
(21) 



where v n , v^, and vq are the corresponding noise vectors for 
code, phase, and doppler measurements, respectively. 

Note that the noise vectors v v , v^, and are correlated. 
Let Lj be the Cholesky factor of Pj x , i.e., PJ 1 = L^Lj 
where 

2 1 ••• 1 
1 2 ••■ 1 



P.j = 



1 1 



is a M-lxM-1 matrix. One can verify that in information 
array form0 the distribution of v v , v^, and i>£ can be written 



as 



v v ~ [LjUr,0], ~ [LjU$,0], vc,~[LjU D ,0] 

where U R = diag[u R , tt£f], U R = diag[u^, uf], U D = 
diaglw]}, Up], and u R , u%, and u D are defined in (l30b - 
(l32l . respectively. 

dT9]i-j2Tli can be de-correlated by multiplying LjUr, 
LjUcj,, and LjUd on both sides, respectively 



LjUrOr = LjUrErY + v n 

LjU^oq, = LjU^E^y + XLjU^Ja - 
L,;U D o D = LjU D E D y + £> c 



(22) 
(23) 
(24) 



where i> v ~ A/"(0, Im-i)» ^ ~ AT(0, Im-i), and ~ 
AA(0,I M -i). 

IV. Sensor Error Models 

In this section we model the errors generated by IMU, 
speedometer, and GPS measurements using a stochastic 
model. Errors of IMU and vehicle velocity measurements are 
modeled by first-order Gaussian Markov stochastic process. 

3 The information array is an alternative representation of the Gaussian 
distribution. Rather than using the mean and covariance as the parameters 
of a Gaussian distribution (i.e., Af(x; fi, £)), we instead parameterize in the 
square root of the information matrix X = S _ 1 = RR T and the normalized 
information vector z = Rfi, i.e., p(x) ~ [R, z]. 



The errors of pseudo-range measurements are zero-mean 
Gaussian distribution with the variance being a function of 
signal-to-noise ratio (SNR). The errors of phase and Doppler 
measurements are zero-mean Gaussian distribution, and the 
variance depends on satellite's elevation angle. 

A. IMU Sensor Errors 

All measurement from sensors is degraded because of 
errors. The primary sources of errors for IMU sensors are 
bias, scale factor, and measurement noise. Some errors are 
contributed from deterministic process and can be corrected 
through specific bench-calibration procedures, while the other 
errors are not deterministic and need to be modeled by a 
stochastic process. The accelerometer or gyro measurement 
can be expressed as 

s(t) = (1 + S)S(t) + p(t) 

where s and s are the true value of the quantity to be measured 
and the sensor's measured output, respectively; S is the scale 
factor error; and (3(t) is the bias. 

The bias term (3(t) can be decomposed as the following 
two terms: 

p{t) = p + pi{t) 

where Pq represents the time-invariant component, and Pi (t) 
represents the time varying component. Pq is usually specified 
on IMU sensor data sheets as the "turn-on to turn-off" bias 
variation. 

The time varying component Pi (t) is typically model as a 
first-order Gaussian Markov stochastic process [0, E], which 
is expressed as the following ordinary differential equation: 



1 



Pi = --pi + o-pw 



(25) 



where erg is the standard deviation of random walk specified 
in the sensor's data sheet, and w is a Gaussian distribution, 
i.e., w^Af (0,1). 

The discrete version of (EBT l can be expressed as 

— )pi(t) +a l3 v r Kiw (26) 



Pi(t + 1) = (1 )Pi(t) + apVAtw 

T 

where Pi(t + 1) and Pi(t) are the bias at time step t+1 and t, 
respectively, and At is time interval between two contiguous 
steps. 

For most survey-grade IMU sensors, we note that the time 
constant r ^> At, and the term in (|26| | related to r can be 
neglected. Adding Pq to the both sides of (126t . we have the 
process equation for sensor bias as 



P(t+1) = P(t)+afjVAtw 



(27) 



Similarly, we have the process equation for scale factor as 

S(t + 1) = S(t) + a s v r Atw (28) 

where ag the sum of standard deviation of scale factor error 
in the sensor's data sheet 



B. Speedometer Measurement Error 

The speedometer measurement of the land vehicle can be 
measured by wheel encoders. 



vh =v H + S v v H + fiv 



(29) 



where vjj and vjj are the true ground and measured vehicular 
ground velocity, respectively; /3 V is the random walk term 
modeling the measurement bias; and S v is the scale factor of 
the velocity measurement. 



C. GPS Measurement Errors 

The three GPS measurements of pseudo-range, phase, and 
doppler from a satellite can be modeled as ([T4l>-([T6l> where 
^AB' Olb> anc ' Cab are tne error terms, respectively. 

1) Pseudorange error rj^g.- The single difference of pseu- 
dorange error for the a-th satellite, rj A a B can be expressed as 



A. GPS Measurement Equation 

We have expressed the unknown state vector y in e-frame 
in Section [Til] However, the local geodetic coordinate system 
(?i-frame in Section HJi with the reference receiver ya as the 
origin is more appropriate to integrate with data from IMU 
and in-vehicle sensor. 

Let reps = ^?"b and vgps = R"i> where the rotation 
matrix R™ is defined in (Q]). 

Usually IMU center and GPS antenna are not placed at the 
same position on the vehicle. This spatial separation causes 
the IMU and GPS measurements to be slightly different in 
position and velocities. This effect is called level-arm effect 
and can be modeled as the following equation position 



reps = r 



(33) 



where r™ is the position of the IMU center in dl3l . 

Taking derivative of (l33l l with respect to time, we obtain 



vgps 



R?,Arlt 



(a) (a) 

Tab = V B 



(a) 



Using the variance model of lfl4l . we model rjf' W = A 
or B) as a zero-mean Gaussian distribution rfh^ ~ A/"(0, C ■ 

snrJ^ 

10 T °~) where the variance is a function of signal-to-noise 
ratio SNR.,3 and C = 0.7 • 10 5 m 2 . Assuming ry^ and 
rpT are independent, we can write the variance of r}^ B as 



C ■ (10' 



10" 



"), and the distribution of r) A a B m 



information array form can be written as 



(a) 
VAB 



(30) 



where u R = 



C(10" 



"+1CT 



2) Phase error We model the phase error t; A a B ~ 

Af(0, sh °i* E ) where E a is the elevation angle of the satellite 
a, and er| is the measurement variance. In information array 
form, the distribution is 



where u$ 



sin E a 



A<*) 

MB 



3) Doppler error Cab- We m °del the Doppler measure- 
ment error C, A a B ~ A/"(0, si °f E ) with a 2 D being the variance 
for doppler measurement. In information array form, the 
distribution is 



K,o] 



(31) 



^AB 



1 D J 



01 



(32) 



where u% = 



V. IMU and GPS Integration 

In this section, we provide the implementation details how 
to integrate data from IMU, GPS, and speedometer using BN 

na. 



where R™ is computed in ( fTTT i and v n is the velocities of the 
IMU center in ([T3l . 

In this paper we assume Ar£ A is known by surveying. 

We define the augmented state vector x consisting of 
kinematic vector of the IMU center in n-frame and terms 
for compensating bias and scale factor, i.e., 

x= [ r" v" 6 n v (3 f /3 U S f S w ] T 

where 0™ is the vehicle's attitude with respect to n-frame, 
(3 j = (bf x ,bf y ,bf z ) T are the bias terms for the accelerom- 
eters along x-, y-, and z-axis and /3 U = {b UJ4> ,b Uf) ,b LUil ) T 
for gyros rates along x-, y-, and z-axis in the vehicle 
frame, respectively; and Sf = (S 'f x , Sf y , S f z ) T and S w = 
{SvAfSue, "S'w v ,) T are the scale factors for the corresponding 
measurements by the IMU sensors. 

Therefore we can express GPS antenna state vector y in 
e-frame by the augmented state x 



Tx 
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a zero- valued matrix with size of 3 x 3. 
Let 
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We can write measurement equations 

o=[ H x H a ] 



in short as 



(34) 



where the de-correlated noise vector vq = (z>^,^,^) T 
distributed as vq ~ [I 3 m- 3 ,0]. 



is 



B. State Process Equation 

Referring (l27t , and (l28l l. we have 

P = P + S/P + (3 f 
< = + S u &! + /3 U 

where P and u)\ are the actual sensor readings of acceleration 
and angular rate, respectively. Plugging above two equations 
into the discrete version of ([L31 . we obtain the system process 
equation d35l l where x t+ i and x f is the state vector at time 
steps t + 1 and t, respectively; [P] and [£>"] are matrices 
whose diagonal entries are P and respectively; At is the 
duration between two consecutive two steps; k r , k„, and k w 
are random vectors of zero-mean Gaussian distributions that 
models the un-modeled uncertainties (e.g., time jitters and 
errors from model parameters) in (Q~3); crp f , crp u , o~p s , and 



o~s w are sensor error parameters defined in Table I; random 
vectors w (3/ ~ JV(0,I 3 ), vtp m ~ A/"(0,I 3 ), w S/ ~JV(0,I 3 ), 
and wj u ~ AT(0, 1 3 ); and I 3 is a 3 x 3 identity matrix. 

TABLE I 

Error model parameters for CrossBow Fiber optical gyro 

SYSTEM FG700AB AND SPEEDOMETER, cr^ AND ag u ARE THE 
RANDOM WALK PARAMETERS FOR ACCELERATION AND ANGULAR RATE, 

RESPECTIVELY. CT S/ AND (J Sij ARE THE SCALE FACTOR PARAMETERS 
FOR ACCELERATION AND ANGULAR RATE, RESPECTIVELY. <Tp v AND (?s v 
ARE THE ERROR PARAMETERS OF SPEEDOMETER. 
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C. LVNS Velocity Constraint 

We further augment the state vector x with the error two 
parameters /3 V and S v (c.f., Section ITV-Bb for vehicle velocity 
measurement. The process equations for j3 v and S v can be 
modeled similarly as in Eq. (l27l i and (l28l l. respectively. We 
have 

/8»(t+l) \ = f Pv(t) \ f af, v VAlw 0v 
S v (t + l) J \ S v (t) J + { a Sv VAlw Si , 

where wp v ~ A/"(0, 1) and ws„ ~ A/"(0, 1) are Gaussian 
random variables; erg,, and ct^^ are the parameters of standard 
deviation specified in Table I. 

Note that d35l l and ( |36l l can be combined and normalized 
to be 

x t+1 = F t x t + u t + w t (37) 

where w t — W(0,I). 

Assuming the land vehicle does not slip and travels along 
the bore-sight of the vehicle (i.e., a;-axis in w-frame), we 
have the vehicular velocity to be zero along the directions 
perpendicular to x-axis in w-frame, i.e., v* = (vjj ,0,0) T ■ 
Let the vehicle measurement v v = (vh,0,0) t and vr is 
directly measured from the speedometer. Using (|29^ we have 

(v H \ / 1 \ 

v" = R v n v n +1 I S v + I I (3 V + vy (38) 

where Vy ~ Af(0, E„) is the random vector denoting un- 
modeled disturbances. 



Note that the above equation can be merged into the 
measurement equation d34l l. 

D. Vehicle Trajectory Reconstruction 

Here we discuss the post-mission data process. The objec- 
tive is to obtain an optimal estimate for the vehicle trajectory 
and the GPS ambiguities a, given all measurements available 
to us. By Xt and o t we denote the augmented vehicle state 
and the measurement observation at discrete time step t, 
respectively. Let the entire vehicle trajectory as Xi :T = [x 4 ] T 
(up to epoch t), all the ambiguities as a, and all t he 
measurements as Oi :T = [o t ] T (up to epoch r). Given Oi :r , 
we use the Bayesian network (BN) to compute the maximum 
likelihood estimate of Xi :T and a: 

max p(Xi :T ,a | Oi :T ) 

Xi :I .a 

Fig. |3] shows the block diagram of the proposed vehicular 
positioning system. The motion prediction module monitors 
the inputs from the IMU sensor, the gravity estimator, and 
the previous state vector x t using (f3Tb . The output from 
the motion prediction module x t+ i is updated based on the 
data from GPS observations (i.e., pseudo-range measurement 



B-ab ' P nase measurement and Doppler measurement 

D® B ), the vehicle speed from speedometer (i.e., Xh), and 
the known level-arm Ai"lv- The measurement update module 
computes the new estimate of the state x t+ i, and phase 
measurement ambiguity vector a. 
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Fig. 3. Block diagram of IMU and GPS integration for LVNS. 
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Fig. 4. Flowchart diagram of IMU and GPS integration for LVNS. 

Fig. |4] shows the flowchart of the proposed positioning 
system. Since the data refreshing rates between IMU and 
GPS/speedometer are different and not synchronized, we use 
the event driven structure to process the data. The motion 
prediction module is called whenever new data from IMU 
sensor is arrived. Measurement update using d34t and (1381 are 
triggered once a new measurement from GPS and speedome- 
ter. 

VI. Conclusions and Future Work 
We have described the implementation details of the 
positioning system that integrates GPS measurements (i.e., 
pseudo-range, carrier-phase and doppler), IMU measure- 
ments, and speedometer measurements. We derived the state 
process equation for motion prediction, the GPS measurement 
equation, and speedometer measurement equation. From these 
linearized equations, the techniques of extended Kalman 
filtering (EKF) or BN can be applied to jointly estimate the 
vehicle trajectory and the phase ambiguity. 
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